/*
 * @Author: AlanLee
 * @Date: 2024-08-20 16:14:24
 * @LastEditors: Sian Li lisian_magic@163.com
 * @LastEditTime: 2024-09-14 21:49:26
 * @FilePath: /hudar_verge/src/hudar_core/src/radar_front.cpp
 * @Description:
 */

#include "Detectors/Detector.h"
#include "hudar_msg/msg/raw_locations.hpp"
#include <cv_bridge/cv_bridge.h>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <thread>
#include <vector>
using namespace cv;
class RadarFront : public rclcpp::Node {
  public:
    RadarFront() : Node("radar_front") {
        cannyMap = cv::imread("properties/cannyMap.png");

        if (cannyMap.empty()) {
            RCLCPP_INFO(this->get_logger(),
                        "cannyMap Can Not Be Found! Exiting!");
            exit(-1);
        }

        loc_publisher = this->create_publisher<hudar_msg::msg::RawLocations>(
            "/radar/raw_locations", 10);

        this->carDetector = std::make_shared<CarDetector>(
            "properties/models/car.engine", "car.onnx");
        this->armorDetector = std::make_shared<ArmorDetector>(
            "properties/models/armor.engine", "armor.onnx");
        carDetector->accessModelTest();
        if (!carDetector->initModel()) {
            RCLCPP_INFO(this->get_logger(),
                        "CarDetector Initialization Failed!");
            exit(-1);
        }
        armorDetector->accessModelTest();
        if (!armorDetector->initModel()) {
            RCLCPP_INFO(this->get_logger(),
                        "ArmorDetector Initialization Failed!");
            exit(-1);
        }
        cv::namedWindow("radar_front", cv::WINDOW_FREERATIO);
        cv::namedWindow("result", cv::WINDOW_FREERATIO);
        cv::resizeWindow("radar_front", 1600, 1200);
        cv::resizeWindow("result", 1000, 600);
        cv::setMouseCallback("radar_front", &RadarFront::onMouse, this);
        // 创建订阅者，订阅"/image_raw"话题
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/radar/image_raw", 10,
            std::bind(&RadarFront::image_callback, this,
                      std::placeholders::_1));
    }
    void spin() {
        cv::Mat curFrame = frame.clone();
        hudar_msg::msg::RawLocations send_msg;
        if (!curFrame.empty()) {
            vector<DetectBox> sepTargets = carDetector->infer(curFrame);
            vector<bboxAndRect> pred =
                armorDetector->infer(curFrame, sepTargets);
            // 画出车框
            for (DetectBox &it : sepTargets) {
                cv::rectangle(curFrame,
                              Rect(it.x1, it.y1, it.x2 - it.x1, it.y2 - it.y1),
                              Scalar(0, 255, 0), 2);
            }

            drawArmorsForDebug(pred, curFrame);
            if (pickPointsReady) {

                for (bboxAndRect &it : pred) {
                    float xx = (it.armor.x0 + it.armor.w / 2);
                    float yy = (it.armor.y0 + it.armor.h);
                    circle(frame, cv::Point(xx, yy), 25, cv::Scalar(0, 255, 0),
                           5);
                    cv::Mat cameraPoint = (cv::Mat_<double>(1, 3) << xx, yy, 1);
                    cameraPoint = cameraPoint.reshape(1, 3);

                    cv::Mat transLoc = trans1 * cameraPoint;
                    transLoc /= transLoc.at<double>(0, 2);
                    double xxx = transLoc.at<double>(0, 1);
                    double yyy = transLoc.at<double>(0, 0);

                    if (isPointInsideImage(cannyMap, int(yyy), int(xxx))) {
                        cout << "xxx: " << xxx << " yyy: " << yyy << endl;
                        // R型高地
                        Mat polygon1 = (Mat_<int>(4, 2) << 121, 1782, 467, 1974,
                                        434, 2244, 121, 2240);
                        Mat polygon2 = (Mat_<int>(4, 2) << 1126, 23, 1495, 14,
                                        1481, 662, 1112, 415);
                        // 环形高地
                        Mat polygon3 =
                            (Mat_<int>(6, 2) << 350, 1292, 626, 1465, 869, 1470,
                             962, 1624, 565, 1675, 228, 1442);
                        Mat polygon4 =
                            (Mat_<int>(6, 2) << 710, 434, 1107, 420, 1476, 723,
                             1308, 840, 1042, 662, 733, 648);
                        if (isPointInPolygon(polygon1, Point(yyy, xxx)) ||
                            isPointInPolygon(polygon2, Point(yyy, xxx))) {
                            // 套用trans3进行仿射变换
                            cv::Mat transLoc = trans3 * cameraPoint;
                            transLoc /= transLoc.at<double>(0, 2);
                            xxx = transLoc.at<double>(0, 1);
                            yyy = transLoc.at<double>(0, 0);
                        } else if (isPointInPolygon(polygon3,
                                                    Point(yyy, xxx)) ||
                                   isPointInPolygon(polygon4,
                                                    Point(yyy, xxx))) {
                            // 套用trans2进行仿射变换
                            cv::Mat transLoc = trans2 * cameraPoint;
                            transLoc /= transLoc.at<double>(0, 2);
                            xxx = transLoc.at<double>(0, 1);
                            yyy = transLoc.at<double>(0, 0);
                        }
                    }

                    xxx = 2800 - xxx;
                    yyy = 1500 - yyy;

                    send_msg.x_robot[int(it.armor.cls)] = xxx / 100;
                    send_msg.y_robot[int(it.armor.cls)] = yyy / 100;

                    char loc_str[20];
                    sprintf(loc_str, "x: %.2f, y: %.2f", xxx / 100, yyy / 100);
                    cv::putText(
                        curFrame, loc_str, cv::Point(it.armor.x0, it.armor.y0),
                        cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 3);
                }
                // 发送坐标消息
                this->loc_publisher->publish(send_msg);
            }
            if (ptsCount >= 12 && !pickPointsReady) {
                // 计算仿射变换矩阵
                Point2f src1[4] = {pts[0], pts[1], pts[2], pts[3]};
                Point2f src2[4] = {pts[4], pts[5], pts[6], pts[7]};
                Point2f src3[4] = {pts[8], pts[9], pts[10], pts[11]};
                trans1 = getPerspectiveTransform(src1, opts1);
                trans2 = getPerspectiveTransform(src2, opts2);
                trans3 = getPerspectiveTransform(src3, opts3);
                // 输出仿射变换结果
                cv::Mat output;
                cv::warpPerspective(curFrame, output, trans1,
                                    cv::Size(1500, 2800));
                cv::Mat res;
                cv::addWeighted(output, 0.5, cannyMap, 0.5, 0, output);
                cv::imwrite("log/result1.jpg", output);
                cv::resize(output, output, cv::Size(500, 800));
                res = output;
                cv::warpPerspective(curFrame, output, trans2,
                                    cv::Size(1500, 2800));
                cv::addWeighted(output, 0.5, cannyMap, 0.5, 0, output);
                cv::resize(output, output, cv::Size(500, 800));
                cv::hconcat(res, output, res);
                cv::warpPerspective(curFrame, output, trans3,
                                    cv::Size(1500, 2800));
                cv::addWeighted(output, 0.5, cannyMap, 0.5, 0, output);
                cv::resize(output, output, cv::Size(500, 800));
                cv::hconcat(res, output, res);

                cv::imshow("result", res);
                cv::imwrite("log/result2.jpg", res);
                pickPointsReady = true;
                RCLCPP_INFO(this->get_logger(), "PickPoints Done!");
            }
            drawAllPickedPoints(curFrame);
            cv::imshow("radar_front", curFrame);
            if (cv::waitKey(1) == 27) {
                carDetector->unInit();
                armorDetector->unInit();
                rclcpp::shutdown();
            }
        }
    }

  private:
    // 判断一个点是否在一个由多个点围成的多边形里边
    bool isPointInPolygon(const Mat &polygon, const Point &point) {
        // 将Mat类型的多边形顶点转换为vector<Point>
        vector<Point> contour;
        for (int i = 0; i < polygon.rows; i++) {
            contour.push_back(
                Point(polygon.at<int>(i, 0), polygon.at<int>(i, 1)));
        }

        // 使用pointPolygonTest函数判断点是否在多边形内部
        double result = pointPolygonTest(contour, point, false);

        // 如果result >= 0，点在多边形内部或边界上
        return result >= 0;
    }
    bool isPointInsideImage(const cv::Mat &image, int x, int y) {
        return (x > 0 && x < image.cols && y > 0 && y < image.rows);
    }
    void drawArmorsForDebug(vector<bboxAndRect> &armors, Mat &img) {
        for (auto &it : armors) {
            Rect temp = Rect(it.armor.x0, it.armor.y0, it.armor.w, it.armor.h);
            cv::rectangle(img, temp, Scalar(0, 255, 0), 2);
            cv::putText(img, to_string(int(it.armor.cls)),
                        cv::Point2i(temp.x, temp.y), cv::FONT_HERSHEY_SIMPLEX,
                        1, cv::Scalar(0, 0, 255), 3);
        }
    }
    void drawAllPickedPoints(cv::Mat &frame) {
        int cnt = 1;
        for (const cv::Point p : pts) {
            putText(frame, std::to_string(cnt++), p, FONT_HERSHEY_SIMPLEX, 2,
                    Scalar(0, 0, 255), 3, 8, false);
            circle(frame, p, 5, cv::Scalar(0, 255, 0), 5);
        }
    }

    // 鼠标回调函数
    static void onMouse(int event, int x, int y, int flags, void *userdata) {

        RadarFront *self = static_cast<RadarFront *>(userdata);
        if (self->ptsCount >= 12)
            return;
        if (event == cv::EVENT_LBUTTONDOWN) {
            // RCLCPP_INFO(self->get_logger(), "Left button clicked at (%d,
            // %d)",
            //             x, y);
            self->pts.push_back(cv::Point(x, y));
            self->ptsCount++;
        }
    }
    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
        std::lock_guard<std::mutex> lock(mutex_);
        try {
            // 使用cv_bridge将ROS图像消息转换为OpenCV格式
            cv::Mat tmp = cv_bridge::toCvShare(msg, "bgr8")->image;
            if (tmp.empty())
                return;
            frame = tmp.clone();
        } catch (cv_bridge::Exception &e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s",
                         e.what());
            return;
        }
    }
    rclcpp::Publisher<hudar_msg::msg::RawLocations>::SharedPtr loc_publisher;
    std::shared_ptr<CarDetector> carDetector;
    std::shared_ptr<ArmorDetector> armorDetector;
    // 地图上像素取点
    Point2f opts1[4] = {Point2f(189, 1076), Point2f(1240, 1767),
                        Point2f(797, 2323), Point2f(513, 2269)};
    Point2f opts2[4] = {Point2f(689, 927), Point2f(949, 929),
                        Point2f(797, 1773), Point2f(588, 1772)};
    Point2f opts3[4] = {Point2f(1122, 740), Point2f(1218, 805),
                        Point2f(386, 2048), Point2f(293, 1983)};
    // 现场取点获得的仿射变换矩阵
    Mat trans1;
    Mat trans2;
    Mat trans3;
    // 二值化地图，用于验证
    cv::Mat cannyMap;

    bool pickPointsReady = false;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
    std::vector<cv::Point> pts;
    cv::Mat frame;
    std::mutex mutex_;
    int ptsCount = 0;
};

void spin_thread(rclcpp::Node::SharedPtr node) { rclcpp::spin(node); }

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    std::shared_ptr<RadarFront> node = std::make_shared<RadarFront>();
    std::thread ros_thread(spin_thread, node);
    while (rclcpp::ok()) {
        node->spin();
    }

    // 将spin放到thread里运行，防止图像获取回调函数阻塞线程导致鼠标回调函数无法运行。
    ros_thread.join();
    rclcpp::shutdown();
    return 0;
}
